package com.grt192.sensor;

import java.util.Vector;

import com.grt192.core.Sensor;
import com.grt192.event.component.GyroEvent;
import com.grt192.event.component.GyroListener;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.PIDSource;

/**
 * GRTGyro is a continuously running sensor driver that collects and serves data
 * from a single-axis gyro
 * 
 * @author anand
 */
public class GRTGyro extends Sensor implements PIDSource {

    public static final double SPIKE_THRESHOLD = 2.0;
    public static final double CHANGE_THRESHOLD = .1;
    private Gyro gyro;
    private Vector gyroListeners;
    public static double DRIFT_THRESHOLD = 0.5;

    public GRTGyro(int channel, int pollTime, String id) {
        gyro = new Gyro(channel);
        setSleepTime(pollTime);
        setState("Angle", 0.0);
        gyroListeners = new Vector();
        this.id = id;
    }

    public void poll() {
        double previousValue = getState("Angle");
        double angle = gyro.getAngle();

        setState("Angle", angle);

        if (Math.abs(getState("Angle") - previousValue) >= SPIKE_THRESHOLD) {
            notifyGyroSpike();
        }
        if (Math.abs(getState("Angle") - previousValue) >= CHANGE_THRESHOLD) {
            notifyGyroChange();
        }
        notifyGyroListeners();
    }

    protected void notifyGyroSpike() {
        for (int i = 0; i < gyroListeners.size(); i++) {
            ((GyroListener) gyroListeners.elementAt(i)).didAngleSpike(new GyroEvent(this, GyroEvent.SPIKE,
                    getState("Angle")));
        }
    }

    protected void notifyGyroChange() {
        for (int i = 0; i < gyroListeners.size(); i++) {
            ((GyroListener) gyroListeners.elementAt(i)).didAngleChange(new GyroEvent(this, GyroEvent.CHANGE,
                    getState("Angle")));
        }
    }

    protected void notifyGyroListeners() {
        for (int i = 0; i < gyroListeners.size(); i++) {
            ((GyroListener) gyroListeners.elementAt(i)).didReceiveAngle(new GyroEvent(this, GyroEvent.NEW_DATA,
                    getState("Angle")));
        }
    }

    public Vector getGyroListeners() {
        return gyroListeners;
    }

    public void addGyroListener(GyroListener a) {
        gyroListeners.addElement(a);
    }

    public void removeGyroListener(GyroListener a) {
        gyroListeners.removeElement(a);
    }

    public String toString() {
        return "gyro: " + getState("Angle");
    }

    public double pidGet() {
        // fix asap
        return 0;
    }
}
